#include <math.h>
#include "distances.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
//#include "std_msgs/Header.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float64MultiArray.h"
#include "pi_tracker/Skeleton.h"
#include "distances/DistancesMatrix.h"
#include <vector>
#include <iostream>
#include <opencv2/opencv.hpp>
#include "st.h"
#include <sstream>
#include "IndList.h"
#include "Display.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
//#include "/opt/ros/fuerte/stacks/vision_opencv/cv_bridge/include/cv_bridge/cv_bridge.h"
#include <sensor_msgs/image_encodings.h>
#include <opencv/highgui.h>

int sjoints[] = {0, 3, 2, 2, 2, 2, 3, 6, 4, 7, 9, 9, 12, 10, 13};
int ejoints[] = {1, 6, 3, 6, 9, 12, 4, 7, 5, 8, 12, 10, 13, 11, 14};
int bonesIndex[2][15] = {
  {0, 3, 2, 2, 2, 2, 3, 6, 4, 7, 9, 9, 12, 10, 13},
  {1, 6, 3, 6, 9, 12, 4, 7, 5, 8, 12, 10, 13, 11, 14}
};
int amtime;
int amtime2;
ros::NodeHandle *pn;
ros::Publisher *pp;

Mat testmatrix;
std::string numberOfIndividuals;
int interval;

int isec;
int insec;
int secs2;
int secs1;
int nsecs2;
int nsecs1;

int ut;
int rec = 1;


bool imagePublished;
bool skeletonPublished;

class itemClass {
public:
  float matrix[15][15];
  int sec;
  int nsec;
  
};



itemClass item;
std::vector<itemClass> ameanvector;
std::vector< std::vector<float> > bonesSamples;
float amean[15][15];

IndList LOI;

char act = 'P';
static const char WINDOW[] = "Image window";
Display display;

void ImageCallBack(const sensor_msgs::ImageConstPtr& msg) {
  //std::cout << "image"<<std::endl;
  
  imagePublished = true;
  namespace enc = sensor_msgs::image_encodings;
  cv_bridge::CvImagePtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvCopy(msg, "");
  }    catch (cv_bridge::Exception& e) {
    //         ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  display.camera=cv_ptr->image;
  std::cout << "image" << std::endl;
  
  
  
  //cv::namedWindow(WINDOW);
  //cv::imshow(WINDOW, cv_ptr->image);
  //cv::waitKey(3);
}

void Callback(const pi_tracker::Skeleton::ConstPtr &msg) {
  std::cout << "skeleton"<<std::endl;
  skeletonPublished = true;
  //passa o valor do impq::imp();parametro amtime pra variável int amtime2
  //std::cout << "Chegou esqueleto" << std::endl;
  ros::param::param("amtime", amtime2, 1000);
  //std::cout << amtime2 << std::endl;
  
  float distances[15][15];
  
  using namespace nsJoints;
  
  distances::DistancesMatrix matrix;
  //distances::DistancesMatrix ameanmatrix;
  distances::Vector vector;
  
  
  
  matrix.dM.clear();
  
  for (int i = 0; i < 15; i++) {
    vector.v.clear();
    for (int j = 0; j < 15; j++) {
      distances[i][j] = skeletonDistance(msg, i, j);
      item.matrix[i][j] = distances[i][j];
      vector.v.push_back(distances[i][j]);
    }
    matrix.dM.push_back(vector);
    
  }
  secs2 = (int) msg->header.stamp.sec;
  nsecs2 = (int) msg->header.stamp.nsec;
  item.sec = secs2;
  item.nsec = nsecs2;
  ameanvector.push_back(item);
  
  std::vector<float> bones;
  for (int i = 0; i < 15; i++) {
    bones.push_back(distances[bonesIndex[0][i]][bonesIndex[1][i]]);
  }
  
  bonesSamples.push_back(bones);
  
  
  //ROS_INFO("an Instantaneous distance matrix was just calculated!");
  
  int intervalsec = ameanvector.back().sec - ameanvector.front().sec;
  int intervalnsec = ameanvector.back().nsec - ameanvector.front().nsec;
  int ameaninterval = intervalsec * 1000 + intervalnsec / 1000000;
  
  //std::cout<<"the amean interval is"<<ameaninterval<<std::endl;
  
  if (amtime2 < amtime) {
    
    
    while (ameaninterval > amtime2) {
      ameanvector.erase(ameanvector.begin());
      bonesSamples.erase(bonesSamples.begin());
      intervalsec = ameanvector.back().sec - ameanvector.front().sec;
      intervalnsec = ameanvector.back().nsec - ameanvector.front().nsec;
      ameaninterval = intervalsec * 1000 + intervalnsec / 1000000;
    }
    
  }
  amtime = amtime2;
  if (ut == 1) {
    secs1 = (int) msg->header.stamp.sec;
    nsecs1 = (int) msg->header.stamp.nsec;
    ut = 0;
  }
  
  
  isec = secs2 - secs1;
  insec = nsecs2 - nsecs1;
  
  interval = isec * 1000 + insec / 1000000;
  
  if (interval > amtime2) {
    ameanvector.erase(ameanvector.begin());
    bonesSamples.erase(bonesSamples.begin());
    secs1 = ameanvector.front().sec;
    nsecs1 = ameanvector.front().nsec;
  }
  
  for (int k = 0; k < ameanvector.size(); k++) {
    for (int i = 0; i < 15; i++) {
      for (int j = 0; j < 15; j++) {
	amean[i][j] = ameanvector.at(k).matrix[i][j] + amean[i][j];
      }
    }
  }
  matrix.mM.clear();
  
  for (int i = 0; i < 15; i++) {
    vector.v.clear();
    for (int j = 0; j < 15; j++) {
      amean[i][j] = amean[i][j] / ameanvector.size();
      vector.v.push_back(amean[i][j]);
    }
    matrix.mM.push_back(vector);
  }
  
  float vmean[15];
  for (int i = 0; i < 15; i++) {
    vmean[i] = amean[bonesIndex[0][i]][bonesIndex[1][i]];
  }
  matrix.quant = ameanvector.size();
  matrix.interval = interval;
  //ROS_INFO("an Avarage Mean matrix was just calculated!");
  pp->publish(matrix);
  
  for (int i = 0; i < 15; i++) {
    for (int j = 0; j < 15; j++) {
      item.matrix[i][j] = 0;
      amean[i][j] = 0;
      
    }
    
    
    
    //adiciona esqueleto e estatisticas à imagem
    
    Stat st;
    float stDev[15];
    st.stDev(bonesSamples, stDev);
    //std::cout<<"208"<<std::endl;
    display.drawSkeleton(msg, bonesIndex, LOI, distances, stDev, vmean);
    
    
    //display.showWindow();
    //std::cout<<"210"<<std::endl;
    
    
    
    //distância do sensor ao chão em metros
    //sensorHeight = 1;
    
    
    
    LOI.collectSamples(act, distances, 15, bonesIndex, 100);
    //não salvar sempre
    LOI.saveToFile("/home/dalton/ros_workspace/distances/DataSets/DataSet.xml");
    
    //std::cout << "wK = " << wK << std::endl;
    //std::cout << "act = " << act << std::endl;
    
    
  }
  
}



using namespace cv;

int main(int argc, char **argv) {
  
  using namespace std;
  
  LOI.loadFromFile("/home/dalton/ros_workspace/distances/DataSets/DataSet.xml");
  
  
  //LOI.Individuals.clear();
  
  /**right
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command
   * line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the
   * easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "distances");
  
  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the
   * last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;
  pn = &n;
  
  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the
   * Subscriber
   * object go out of scope, this callback will automatically be unsubscribed
   * from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to
   * throw
   * away the oldest ones.
   */
  
  
  for (int i = 0; i < 15; i++) {
    for (int j = 0; j < 15; j++) {
      item.matrix[i][j] = 0;
      amean[i][j] = 0;
      
    }
  }
  
  
  ut = 1;
  ros::Subscriber skeletonTopic = n.subscribe("skeleton", 1000, Callback);
  //ros::Subscriber sub = n.subscribe("/skeleton", 1000, ftest);
  ros::Subscriber imageTopic = n.subscribe("camera/rgb/image_rect_color", 10, ImageCallBack);
  
  ros::Publisher pub = n.advertise<distances::DistancesMatrix > ("distances", 1000);
    pp = &pub;
  
  ros::Rate r(30); // 10 hz
  
imagePublished=true;
skeletonPublished=true;
  
  
  while(ros::ok()){
    
    
 
    
    char wK = display.showWindow(imagePublished,skeletonPublished);
    if (wK == 'R' || wK == 'r') {
      act = 'R';
    }
    
    if (wK == 'E' || wK == 'e') {
      int iD;
      std::cout << "What individual would you like to delete? Type in a number:" << std::endl;
      std::cin >> iD;
      
      LOI.Individuals.erase(LOI.Individuals.begin() + iD);
    }
    
    imagePublished = false;
    skeletonPublished = false;
    
    ros::spinOnce();
    r.sleep();
  }
  
  //ros::Rate loop_rate(10);
  
  
  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one). 
   * ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  
  
  
  
  
  return 0;
}
